TP2 - Filtros de Kalman

Elementos de Matemáticas Aplicadas para Aplicaciones Tecnológicas

Autor/a

Afiliación

Matías Roqueta

Instituto Balseiro

Descripción del problema

Se implementará un filtro de Kalman para fusionar los datos de sensores presentes en un vehículo para estimar la posición y orientación del mismo en dos dimensiones. Un diagrama del mismo se presenta en la Figura 1.

Figura 1: Esquema del sistema.

En la figura se representan los ejes \(x\) e \(y\), llamados el sistema de referencia inercial. Se repesenta además el eje \(x_b\), el cual junto con un eje \(y_b\) ortogonal al mismo forman el sistema de referencia del móvil, llamado body. El filtro de Kalman estimará la posición del móvil en el sistema de referencia intercial, \(x_{est},\, y_{est}\) así como la orientación del mismo, dada por el ángulo \(\theta\).

Los instrumentos disponibles para la estimación de la posición son los siguientes.

• Unidad de Medición Inercial (IMU): Consiste en un giróscopo y un acelerómetro los cuales miden a una tasa \(f_s\).
  • Acelerómetro: Mide la aceleración lineal del móvil en el sistema body, \(u^{x_b} = \ddot{x}_b\) y \(u^{y_b} = \ddot{y}_b\).
  • Giróscopo: Mide la velocidad angular del móvil, \(u^{\theta} = \dot{\theta}\).
• GPS: Mide la posición del móvil en el sistema de referencia inercial a una tasa \(f_s'\).
• Magnetómetro: Por medio de mediciones del campo magnético terrestre calcula la orientación del móvil, también a tasa \(f_s'\).

using LinearAlgebra
using Distributions
using Plots
using PlotThemes

theme(:wong2)

Error de los instrumentos

Para cada dimensión de medición se considera su correspondiente error. Se hace a partir de los datos en la hoja de datos.

Parámetro	Símbolo	Valor
Frecuencia de muestreo IMU	\(f_s\)	\(10 \, \mathrm{Hz}\)
Frecuencia de muestreo GPS	\(f_s'\)	\(1 \, \mathrm{Hz}\)
PSD ruido acelerómetro	\(S_{acc}\)	\(80\, \mathrm{\mu g}/\sqrt{\mathrm{Hz}}\)
PSD ruido giróscopo	\(S_{gir}\)	\(0.03\, ^\circ/s/\sqrt{\mathrm{Hz}}\)
PSD ruido giróscopo	\(S_{gir}\)	\(100\, \mathrm{\mu Gauss}/\sqrt{\mathrm{Hz}}\)
Error absoluto del GPS	\(\sigma_{GPS}\)	\(2.5\, \mathrm{m}\)
Intensidad campo magnético	\(B_T\)	\(25358\,\mathrm{nT}\)

El error de las mediciones del IMU se calculan a partir de la densidad espectral del ruido informada en la hoja de datos, \(S_{acc}\) para el acelerómetro y \(S_{gir}\) para el giróscopo, y la frecuencia de muestreo \(f_s\)

\[ \sigma_{acc} = S_{acc} \sqrt{fs} \qquad \sigma_{acc} = S_{gir} \sqrt{fs} \]

El error del magnetómetro se calcula también a partir de su densidad espectral de ruido \(S_{mag}\) y su frecuencia de muestreo \(f_s'\), y también es afectado por la intensidad del campo magnético terrestre \(B_T\)

\[ \sigma_{mag} = \frac{S_{mag} \sqrt{fs'}}{B_T} \]

Fialmente, el error del GPS no depende de la frecuencia de muestreo, y es informado diréctamente en la hoja de datos

Δt = 0.1

fs = 10
fs2 = 1
PSD_a = 80*10*1e-6
PSD_g = 0.03*π/180
PSD_m = 100e-6*1e-4
B_T = 25358e-9

err_IMU_x = PSD_a*sqrt(fs)
err_IMU_θ = PSD_g*sqrt(fs)

err_GPS = 2.5
err_MAG = PSD_m*sqrt(fs2)/B_T

Q = [err_IMU_x^2 0 0; 0 err_IMU_x^2 0; 0 0 err_IMU_θ^2]

R = [err_GPS^2 0 0; 0 err_GPS^2 0; 0 0 err_MAG^2]

dist_ruido_IMU = MvNormal([0,0,0], Q)
dist_ruido_GPS = MvNormal([0,0,0], R)

Filtro de Kalman

El filtro de Kalman se hará sobre el estimador del estado de posición, velocidad, y orientación del móvil en el sistema de referencia inercial

\[ \mathbf{\hat{x}}_k = \begin{bmatrix}\hat{x}_k \\ \hat{\dot{x}}_k \\ \hat{y}_k \\ \hat{\dot{y}}_k \\ \hat{\theta}_k \end{bmatrix} \]

Medición del IMU

A cada instante de muestreo del IMU se obtiene un vector de mediciones \(\mathbf{\overline{u}}_k\), el cual se define como su valor real \(\mathbf{u}_k\) más su error \(\mathbf{w'}_k\)

\[ \mathbf{\overline{u}}_k = \mathbf{u}_k + \mathbf{w}_k' = \begin{bmatrix}u_k^{x_b} \\ u_k^{y_b} \\ u_k^{\theta} \end{bmatrix} + \mathbf{w}_k' \]

Los errores de medición se consideran independientes, por lo que la matriz de covarianza de \(\mathbf{w}'\) es dada por

\[ Q = \begin{bmatrix} (\sigma_{acc})^2 & 0 & 0\\ 0 & (\sigma_{acc})^2 & 0\\ 0 & 0 & (\sigma_{gir})^2\\ \end{bmatrix} \]

Medición del GPS/Magnetómetro

Cuando \(k\) corresponde a un instante de muestreo del GPS, se obtiene un vector de mediciones \(\mathbf{\overline{z}}_k\), el cual se define como su valor real \(\mathbf{z}_k\) más su error \(\mathbf{v}_k\)

\[ \mathbf{\overline{z}}_k = \mathbf{z}_k + \mathbf{v}_k = \begin{bmatrix}z_k^{x} \\ z_k^{y} \\ z_k^{\theta} \end{bmatrix} + \mathbf{v}_k \]

Los errores de medición se consideran independientes, por lo que la matriz de covarianza de \(\mathbf{w}'\) es dada por

\[ R = \begin{bmatrix} (\sigma_{GPS})^2 & 0 & 0\\ 0 & (\sigma_{GPS})^2 & 0\\ 0 & 0 & (\sigma_{mag})^2\\ \end{bmatrix} \]

Modelo de actualización de estado

En ausencia de aceleración, la evolución del estado en un tiempo \(\Delta t\) seguiría un movimiento rectilíneo uniforme, dado por

\[ \mathbf{x}_k = \begin{bmatrix}x_k \\ \dot{x}_k \\ y_k \\ \dot{y}_k \\ \theta_k \end{bmatrix} = \begin{bmatrix} 1 & \Delta t & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & \Delta t & 0 \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix}x_{k-1} \\ \dot{x}_{k-1} \\ y_{k-1} \\ \dot{y}_{k-1} \\ \theta_{k-1} \end{bmatrix} \]

La actualización del estado se hace en función de las mediciones de la IMU, que informan la aceleración del móvil en el sistema de referencia body, estas se modelan como el vector de medición \(\mathbf u\) más el vector de error \(\mathbf w'\)

Estas aceleraciones se transladan al sistema de referencia inercial y se utilizan para realizar la actualización por modelo del filtro de Kalman, dada por medio de la ecuación

\[ \mathbf{\hat{x}}_k = A\mathbf{\hat{x}}_{k-1} + B_{k-1} \mathbf{u}_{k-1} + \mathbf{w}_{k-1} \]

En donde la matriz \(B_{k-1}\) que obtiene la actualización del estado en el sistema inercial \(\mathbf{x}\) en función de las mediciones de aceleración en el sistema body es dada por

\[ B_{k-1} = \begin{bmatrix} \frac 1 2 \Delta t^2 \cos(\theta_{k-1}) & - \frac 1 2 \Delta t^2 \sin(\theta_{k-1}) & 0 \\ \Delta t \cos(\theta_{k-1}) & - \Delta t \sin(\theta_{k-1}) & 0 \\ \frac 1 2 \Delta t^2 \sin(\theta_{k-1}) & \frac 1 2 \Delta t^2 \cos(\theta_{k-1}) & 0 \\ \Delta t \sin(\theta_{k-1}) & \Delta t \cos(\theta_{k-1}) & 0 \\ 0 & 0 & \Delta t \end{bmatrix} \]

Y el ruido es similarmente transformado, obteniendo \(\mathbf{w}_{k-1} = B \mathbf{w}_{k-1}'\). La matriz de covarianza que resulta de la transformación es dada por

\[ Q_k = B Q B^T \]

Actualización por medición

La actualización por medición se basa en relacionar las mediciones del GPS y el magnetómetro con el estado actual \(\mathbf{\hat{x}}\) de la siguiente forma

\[ \mathbf{z}_k = H\mathbf{\hat{x}}_k - \mathbf{v}_k \]

En donde la matriz \(H\) es dada por

\[ H = \begin{bmatrix} 1 & 0 & 0 & 0 & 0\\ 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 \end{bmatrix} \]

Y el vector de ruido tiene matriz de covarianza \(R\)

A = [1 Δt 0 0 0;
     0 1 0 0 0;
     0 0 1 Δt 0;
     0 0 0 1 0;
     0 0 0 0 1]

function B_matrix(θ)
    return [Δt^2*cos(θ)/2 -Δt^2*sin(θ)/2 0;
    Δt*cos(θ) -Δt*sin(θ) 0;
    Δt^2*sin(θ)/2 Δt^2*cos(θ)/2 0;
    Δt*sin(θ) Δt*cos(θ) 0;
    0 0 Δt]
end

H = [1 0 0 0 0;
     0 0 1 0 0;
     0 0 0 0 1]

Implementación del algoritmo de estimación

El algoritmo de estimación se implementa con una etapa de inicialización del estado \(\mathbf{\hat{x}}\) y de la matriz de covarianza \(P\), seguido de un ciclo for que realiza la predicción en cada instante de muestreo de la IMU, e incluye la etapa de corrección en cada instante de muestreo del GPS.

function kalman(t, tasa_GPS, u, z, Q, R)
    x, P, K = kalman_initialize(t, z, R)
    for k in 2:length(t)
        x_est, P_est = kalman_predict(x[:,k-1], u[:,k], P[:,:,k-1], Q)
        if k%tasa_GPS == 0
            x_est, P_est, K_k = kalman_correct(x_est, z[:,k], P_est, R)
          K[k] = norm(K_k)
        end
        x[:,k] = x_est
        P[:,:,k] = P_est
    end
    return x, P, K
end

kalman (generic function with 2 methods)

Inicialización

Los valores de posición y orientación del estado \(\mathbf{\hat{x}}\) se inicializan con las primeras mediciones provenientes del GPS/magnetómetro. Los valores de velocidad se inicializan en \(0\).

Se decide inicializar la matriz de covarianza con el error del GPS para la posición, el error del GPS multiplicado por la frecuencia de muestreo para la velocidad, y \(2\pi\) para la orientación.

function kalman_initialize(t, z, R)
    N = length(t)
    x = zeros(5, N)
    x[1:2:5,1] = z[:,1]
    P = zeros(5, 5, N)
    P[:,:,1] = diagm([R[1,1]^2,(R[1,1]*fs2)^2,R[2,2]^2,(R[2,2]*fs2)^2,2pi])
    K = zeros(N)
    return x, P, K
end

kalman_initialize (generic function with 1 method)

Predicción

En cada instante de muestreo del IMU se realiza la etapa de predicción

• Estimación a priori: \(\mathbf{\hat{x}}_k^{-} = \mathbf{\hat{x}}_{k-1} + B_{k-1} \mathbf{\overline{u}}_{k-1}\)
• Covarianza a priori: \(P_k^{-} = A P_{k-1}A^T + Q_k\)
• Ganancia de Kalman: \(K_k = P^{-}_k H^T (H P_k^{-}H^T + R_k)^{-1}\)

function kalman_predict(x, u, P, Q)
    B = B_matrix(x[5])
    x_est = A*x + B*u
    P_est = A*P*A' + B*Q*B'
    return x_est, P_est
end

kalman_predict (generic function with 1 method)

Corrección

En cada instante de muestreo del GPS/magnetómetro, se aplica una corrección por medición al estimador.

• Estimador a posteriori: \(\mathbf{\hat{x}}_k^{+} = \mathbf{\hat{x}}_k^{-} + K_k (\mathbf{\overline{z}}_k-H\mathbf{\hat{x}}_k^{-})\)
• Covarianza a posteriori: \(P_k^{+} = (I-K_k H)P_k^{-}\)

function kalman_correct(x_prior, z, P_prior, R)
    K = P_prior*H'*inv(H*P_prior*H'+R)
    x_est = x_prior + K*(z-H*x_prior)
    P_est = (I-K*H)*P_prior
    return x_est, P_est, K
end

kalman_correct (generic function with 3 methods)

Estimación de diferentes trayectorias

En todas las trayectorias, los ruidos de los instrumentos se modelan como variables aleatorias gaussianas de media \(0\), con muestras independientes e idénticamente distribuidas.

\[ \mathbf{w}'_k \sim \mathcal{N}\left[\mathbf{0}, Q\right] \qquad \mathbf{v}_k \sim \mathcal{N}\left[\mathbf{0}, R\right] \]

T = 500
t = 0:0.1:T

u_noise = hcat([rand(dist_ruido_IMU) for ti in t]...)
z_noise = hcat([rand(dist_ruido_GPS) for ti in t]...)

Móvil estático

Se analiza la trayectoria real, dada por \(\mathbf{z}\), contra la trayectoria estimada extraida de \(\mathbf{\hat{x}}\) en el caso de que el vehículo se encuentra en reposo.

En este caso los vectores de medición \(\mathbf{\overline{u}}\) y \(\mathbf{\overline{z}}\) son dados por

\[ \mathbf{\overline{u}}_k = \begin{bmatrix} 0 \\ 0 \\ 0 \end{bmatrix} + \mathbf{w}_k' \qquad\qquad \mathbf{\overline{z}}_k = \begin{bmatrix} 0 \\ 0 \\ 0 \end{bmatrix} + \mathbf{v}_k \]

function estatico_IMU()
    return [0,0,0]
end
function estatico_GPS()
    return [0,0,0]
end

u_true = hcat([estatico_IMU() for ti in t]...)
z_true = hcat([estatico_GPS() for ti in t]...)

u = u_true + u_noise
z = z_true + z_noise

x, P, K = kalman(t, 10, u, z, Q, R)

function trayectoria_xy(x, z_true)
    p1 = plot(x[1,:], x[3,:], label="Estimación", xlabel="x", ylabel="y", aspect_ratio=:equal, title="Trayectoria XY")
    plot!(p1, z_true[1,:], z_true[2,:], label="Trayectoria")
    return p1
end

function trayectoria_xy(x, z_true, k_loss)
    p1 = plot(x[1,:], x[3,:], label="Estimación", xlabel="x", ylabel="y", aspect_ratio=:equal, title="Trayectoria XY")
    plot!(p1, x[1,k_loss], x[3,k_loss], aspect_ratio=:equal, label="Pérdida instrumento")
    plot!(p1, z_true[1,:], z_true[2,:], label="Trayectoria")
    return p1
end

trayectoria_xy(x, z_true)

function trayectoria_t(t, x, z_true)
    p1 = plot(t, x[1,:], ylabel="x [m]", title="Trayectoria en función del tiempo")
    plot!(p1, t, z_true[1,:])
    p2 = plot(t, x[3,:], ylabel="y [m]")
    plot!(p2, t, z_true[2,:])
    p3 = plot(t, x[5,:] .% 2pi, ylabel="θ", xlabel="t [s]")
    plot!(p3, t, z_true[3,:] .% 2pi)
    return plot(p1,p2,p3, layout=(3,1), legend=false)
end

function trayectoria_t(t, x, z_true, k_loss)
    p1 = plot(t, x[1,:], ylabel="x [m]", title="Trayectoria en función del tiempo")
    plot!(p1, t[k_loss], x[1,k_loss])
    plot!(p1, t, z_true[1,:])
    p2 = plot(t, x[3,:], ylabel="y [m]")
    plot!(p2, t[k_loss], x[3,k_loss])
    plot!(p2, t, z_true[2,:])
    p3 = plot(t, x[5,:] .% 2pi, ylabel="θ", xlabel="t [s]")
    plot!(p3, t[k_loss], x[5,k_loss] .% 2pi)
    plot!(p3, t, z_true[3,:] .% 2pi)
    return plot(p1,p2,p3, layout=(3,1), legend=false)
end

trayectoria_t(t, x, z_true)

Movimiento rectilíneo uniformemente acelerado

Se analiza la trayectoria real, dada por \(\mathbf{z}\), contra la trayectoria estimada extraida de \(\mathbf{\hat{x}}\) en el caso de que el vehículo parte del reposo y sigue un movimiento rectilíneo uniformemente acelerado con aceleración \(a\) a un ángulo \(\theta\) respecto a la horizontal.

En este caso los vectores de medición \(\mathbf{\overline{u}}\) y \(\mathbf{\overline{z}}\) son dados por \[ \mathbf{\overline{u}}_k = \begin{bmatrix} a \\ 0 \\ 0 \end{bmatrix} + \mathbf{w}_k' \qquad\qquad \mathbf{\overline{z}}_k = \begin{bmatrix} \frac 1 2 a t_k^2 \cos(\theta) \\ \frac 1 2 a t_k^2 \sin(\theta) \\ \theta \end{bmatrix} + \mathbf{v}_k \]

function mrua_IMU(a)
    return [a,0,0]
end
function mrua_GPS(a, t, θ)
    return [1/2 * a * t^2 * cos(θ), 1/2 * a* t^2 * sin(θ), θ]
end

u_true = hcat([mrua_IMU(5e-4) for ti in t]...)
z_true = hcat([mrua_GPS(5e-4, ti, pi/4) for ti in t]...)

u = u_true + u_noise
z = z_true + z_noise

x, P, K = kalman(t, 10, u, z, Q, R)

trayectoria_xy(x, z_true)

trayectoria_t(t, x, z_true)

Movimiento circular uniforme

Se analiza la trayectoria real, dada por \(\mathbf{z}\), contra la trayectoria estimada extraida de \(\mathbf{\hat{x}}\) en el caso de que el vehículo sigue un movimiento circular uniforme a un radio \(r\) del origen y con velocidad angular \(\omega\).

En este caso los vectores de medición \(\mathbf{\overline{u}}\) y \(\mathbf{\overline{z}}\) son dados por \[ \mathbf{\overline{u}}_k = \begin{bmatrix} 0 \\ \omega^2 r \\ \omega \end{bmatrix} + \mathbf{w}_k' \qquad\qquad \mathbf{\overline{z}}_k = \begin{bmatrix} r \cos(\omega t_k) \\ r \sin(\omega t_k) \\ \omega t_k + \frac \pi 2 \end{bmatrix} + \mathbf{v}_k \]

function circular_IMU(r, w, t)
    return [0,w^2*r,w] 
end

function circular_GPS(r, w, t)
    return [r*cos(w*t),r*sin(w*t),w*t+π/2] 
end

u_true = hcat([circular_IMU(20, 2*pi/T, ti) for ti in t]...)
z_true = hcat([circular_GPS(20, 2*pi/T, ti) for ti in t]...)

u = u_true + u_noise
z = z_true + z_noise

x, P, K = kalman(t, 10, u, z, Q, R)

trayectoria_xy(x, z_true)

trayectoria_t(t, x, z_true)

Análisis de desempeño

En la comparación de las trayectorias reales contra la estimación se observa que el error de medición es máximo al inicio de la trayectoria, y este error disminuye conforme el tiempo avanza y el filtro de Kalman alcanza un estado de equilibrio. Se busca analizar cuantitativamente este efecto comparando el error de la medición y la incerteza de la estimación en función del tiempo.

Error de estimación

Se procede a cuantificar el error de la estimación, el cual se define como la norma cuadrática de la distancia entre la estimación y el valor real

\[ \epsilon_k = \left\lVert\begin{bmatrix}\hat{x}_k\\\hat{y}_k\end{bmatrix}- \begin{bmatrix}z^x_k\\ z^y_k\end{bmatrix}\right\rVert^2 \]

function error_cuadratico(x, z_true)
    xs = [[x_i[1] x_i[3]] for x_i in eachcol(x)]
    zs = [[z_i[1] z_i[2]] for z_i in eachcol(z_true)]
    return [norm(x_i-z_i)^2 for (x_i, z_i) in zip(xs, zs)]
end

error_cuadratico (generic function with 1 method)

Esto se compara con la incerteza del estimador, la cual se cuantifica con la traza de la matriz de covarianza \(P\)

function varianza_estimador(P)
    return [tr(P[:,:,i]) for i in 1:size(P,3)]
end

varianza_estimador (generic function with 1 method)

Los resultados verifican las observaciones, y se presentan a continuación

function plot_error(t, x, z_true, P)
    v = varianza_estimador(P)
    e = error_cuadratico(x, z_true)
    p1 = plot(t, v, ylim=[0, min(100,maximum(v)*1.1)], ylabel="Incerteza")
    p2 = plot(t, e, ylim=[0, 30], ylabel="Error cuadrático", xlabel="t")
    return plot(p1, p2, layout=(2,1))
end

function plot_error(t, x, z_true, P, k_loss)
    v = varianza_estimador(P)
    e = error_cuadratico(x, z_true)
    p1 = plot(t, v, ylim=[0, min(100,maximum(v)*1.1)], ylabel="Incerteza")
    plot!(p1, t[k_loss], v[k_loss], label="Pérdida Instrumento")
    p2 = plot(t, e, ylim=[0, 30], ylabel="Error cuadrático", xlabel="t")
    plot!(p2, t[k_loss], e[k_loss], label="Pérdida Instrumento")
    return plot(p1, p2, layout=(2,1))
end

plot_error(t, x, z_true, P)

Ganancia de Kalman

Además del error de medición, se analiza la evolución de la ganancia de Kalman \(K_k\) en función del tiempo. Esta se cuantifica como la norma de la matriz.

function plot_gain(T, P, K)
    k = findall(x -> x!=0, K)
    v = varianza_estimador(P)
    p1 = plot(t, v, ylabel="Incerteza", linetype=:steppost)
    p2 = plot(t[k], K[k], ylim=[0, maximum(K)*1.1], ylabel="Ganancia Kalman", xlabel="t", linetype=:steppost)
    return plot(p1, p2, layout=(2,1))
end
function plot_gain(t, P1, K1, P2, K2)
    v1 = varianza_estimador(P1)
    v2 = varianza_estimador(P2)
    k1 = findall(x -> x!=0, K1)
    k2 = findall(x -> x!=0, K2)
    p1 = plot(t, v1, ylabel="Incerteza", linetype=:steppost, label=false)
    plot!(t, v2, ylabel="Incerteza", linetype=:steppost, label="Referencia", ylim=(0,100))
    p2 = plot(t[k1], K1[k1], ylim=[0, max(maximum(K1),maximum(K2))*1.1], ylabel="Ganancia Kalman", xlabel="t", linetype=:steppost, label=false)
    plot!(t[k2], K2[k2], ylim=[0, max(maximum(K1),maximum(K2))*1.1], ylabel="Ganancia Kalman", xlabel="t", linetype=:steppost, label="Referencia")
    return plot(p1, p2, layout=(2,1))
end
plot_gain(t, P, K)

Se hace un foco en el estado estacionario del filtro, para observar como es afectada la incerteza del estimador y la ganacia de Kalman en los instantes en los que se registra una medición del GPS.

k1 = 4000
k2 = 4050
plot_gain(t[k1:k2], P[:,:,k1:k2], K[k1:k2])

Efecto de la incerteza de los instrumentos

A continuación se analizará como se modifica el desempeño del filtro si se varían los parámetros de error de los instrumentos que lo componen, ya sea el IMU o el GPS. Se llama caso de referencia al filtro de Kalman desarrollado.

Incremento x5 del error de GPS

Primero se analiza que sucede si el error del IMU se mantiene igual y el error del GPS incrementa 5 veces. Esto se realiza ejecutando el programa pero utilizando la siguiente matriz de covariaza para el ruido de medición

\[ R = \begin{bmatrix} (5\sigma_{GPS})^2 & 0 & 0\\ 0 & (5\sigma_{GPS})^2 & 0\\ 0 & 0 & (\sigma_{mag})^2\\ \end{bmatrix} \]

Se analiza las trayectorias rectilínea y circular, y los resultados se presentan a continuación.

Trayectoria rectilínea

Q = [err_IMU_x^2 0 0; 0 err_IMU_x^2 0; 0 0 err_IMU_θ^2]
R = [(5*err_GPS)^2 0 0; 0 (5*err_GPS)^2 0; 0 0 err_MAG^2]

dist_ruido_IMU = MvNormal([0,0,0], Q)
dist_ruido_GPS = MvNormal([0,0,0], R)

u_noise = hcat([rand(dist_ruido_IMU) for ti in t]...)
z_noise = hcat([rand(dist_ruido_GPS) for ti in t]...)

u_true = hcat([mrua_IMU(5e-4) for ti in t]...)
z_true = hcat([mrua_GPS(5e-4, ti, pi/4) for ti in t]...)

u = u_true + u_noise
z = z_true + z_noise

x, P2, K2 = kalman(t, 10, u, z, Q, R)

trayectoria_xy(x, z_true)

trayectoria_t(t, x, z_true)

Trayectoria circular

u_true = hcat([circular_IMU(20, 2*pi/T, ti) for ti in t]...)
z_true = hcat([circular_GPS(20, 2*pi/T, ti) for ti in t]...)

u = u_true + u_noise
z = z_true + z_noise

x, P2, K2 = kalman(t, 10, u, z, Q, R)

trayectoria_xy(x, z_true)

trayectoria_t(t, x, z_true)

Error de estimación

plot_error(t, x, z_true, P2)

k1 = 4800
k2 = 4850
plot_gain(t[k1:k2], P2[:,:,k1:k2], K2[k1:k2], P[:,:,k1:k2], K[k1:k2])

Observaciones

• El error de estimación es mucho mayor respecto al caso de referencia, en particular cerca del inicio de la trayectoria.
• El filtro tarda mucho más tiempo en alcanzar el estado estacionario respecto al caso de referencia.
• A pesar que la incerteza de medición es mucho mayor, en el estado estacionario la ganancia de Kalman es similar a aquella del caso de referencia.

Incremento x5 del error de IMU

A continuación se analiza que sucede si el error del IMU es el que incrementa 5 veces y el error del GPS se mantiene igual. Esto se realiza ejecutando el programa pero utilizando la siguiente matriz de covariaza para el ruido de modelo

\[ Q = \begin{bmatrix} (5\sigma_{acc})^2 & 0 & 0\\ 0 & (5\sigma_{acc})^2 & 0\\ 0 & 0 & (5\sigma_{gir})^2\\ \end{bmatrix} \]

Trayectoria rectilínea

Q = [(5*err_IMU_x)^2 0 0; 0 (5*err_IMU_x)^2 0; 0 0 (5*err_IMU_θ)^2]
R = [err_GPS^2 0 0; 0 err_GPS^2 0; 0 0 err_MAG^2]

dist_ruido_IMU = MvNormal([0,0,0], Q)
dist_ruido_GPS = MvNormal([0,0,0], R)

u_noise = hcat([rand(dist_ruido_IMU) for ti in t]...)
z_noise = hcat([rand(dist_ruido_GPS) for ti in t]...)

u_true = hcat([mrua_IMU(5e-4) for ti in t]...)
z_true = hcat([mrua_GPS(5e-4, ti, pi/4) for ti in t]...)

u = u_true + u_noise
z = z_true + z_noise

x, P2, K2 = kalman(t, 10, u, z, Q, R)

trayectoria_xy(x, z_true)

trayectoria_t(t, x, z_true)

Trayectoria circular

u_true = hcat([circular_IMU(20, 2*pi/T, ti) for ti in t]...)
z_true = hcat([circular_GPS(20, 2*pi/T, ti) for ti in t]...)

u = u_true + u_noise
z = z_true + z_noise

x, P2, K2 = kalman(t, 10, u, z, Q, R)

trayectoria_xy(x, z_true)

trayectoria_t(t, x, z_true)

Error de estimación

plot_error(t, x, z_true, P2)

k1 = 4000
k2 = 4050
plot_gain(t[k1:k2], P2[:,:,k1:k2], K2[k1:k2], P[:,:,k1:k2], K[k1:k2])

Observaciones

• El error de estimación es mayor respecto al caso de referencia, pero la diferencia no es tan drástica como en el caso que incrementa el error del GPS.
• No se ve afectado el tiempo que tarda el filtro en alcanzar el estado estacionario respecto al caso de referencia.
• El error de estimador es mayor respecto al caso de referencia, incluso en el estado estacionario.
• Se nota que la ganancia de Kalman es mayor respecto al caso de referencia. Esto implica que las mediciones del GPS se consideran más informativas.

Simulación pérdida de instrumentos

Pérdida del GPS

Suponemos la pérdida del GPS. En este caso la ecuación de acualización de estado se convierte en

\[ \mathbf{z}_k' = H'\mathbf{\hat{x}}_k - \mathbf{v}_k' \]

En donde la matriz \(H\) es dada por

\[ H' = \begin{bmatrix} 0 & 0 & 0 & 0 & 1 \end{bmatrix} \]

\[ \mathbf{z}_k' = \begin{bmatrix} z_k^{\theta} \end{bmatrix} \]

Los errores de medición se consideran independientes, por lo que la matriz de covarianza de \(\mathbf{v}'\) es dada por

\[ R' = \begin{bmatrix} (\sigma_{mag})^2\\ \end{bmatrix} \]

Pérdida del magnetómetro

Suponemos la pérdida del GPS. En este caso la ecuación de acualización de estado se convierte en

\[ \mathbf{z}_k'' = H''\mathbf{\hat{x}}_k - \mathbf{v}_k'' \]

En donde la matriz \(H\) es dada por

\[ H'' = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 \end{bmatrix} \]

\[ \mathbf{z}_k'' = \begin{bmatrix} z_k^{x} \\ z_k^{y} \end{bmatrix} \]

Los errores de medición se consideran independientes, por lo que la matriz de covarianza de \(\mathbf{v}'\) es dada por

\[ R'' = \begin{bmatrix} (\sigma_{GPS})^2 & 0\\ 0 & (\sigma_{GPS})^2 \end{bmatrix} \]

function kalman_correct(x_prior, z, P_prior, K, GPS=true, mag=true)
  if GPS && mag
    H2 = H
    z2 = z
    R2 = R
  elseif mag
    H2 = H[3,:]'
    z2 = z[3]
    R2 = R[3,3]
  else
    H2 = H[1:2, :]
    z2 = z[1:2]
    R2 = R[1:2,1:2]
  end   
  K = P_prior*H2'*inv(H2*P_prior*H2'+R2)
  x_est = x_prior + K*(z2-H2*x_prior)
  P_est = (I-K*H2)*P_prior
  return x_est, P_est, K
end

function kalman(t, tasa_GPS, u, z, Q, R, gps, mag)
    x, P, K = kalman_initialize(t, z, R)
    K = zeros(size(t))
    for k in 2:length(t)
        x_est, P_est = kalman_predict(x[:,k-1], u[:,k], P[:,:,k-1], Q)
        if k%tasa_GPS == 0
            x_est, P_est, K_k = kalman_correct(x_est, z[:,k], P_est, R, gps[k], mag[k])
          K[k] = norm(K_k)
        end
        x[:,k] = x_est
        P[:,:,k] = P_est
    end
    return x, P, K
end

kalman (generic function with 2 methods)

Q = [(err_IMU_x)^2 0 0; 0 (err_IMU_x)^2 0; 0 0 (err_IMU_θ)^2]
R = [err_GPS^2 0 0; 0 err_GPS^2 0; 0 0 err_MAG^2]

dist_ruido_IMU = MvNormal([0,0,0], Q)
dist_ruido_GPS = MvNormal([0,0,0], R)

u_noise = hcat([rand(dist_ruido_IMU) for ti in t]...)
z_noise = hcat([rand(dist_ruido_GPS) for ti in t]...)

u_true = hcat([circular_IMU(20, 2*pi/T, ti) for ti in t]...)
z_true = hcat([circular_GPS(20, 2*pi/T, ti) for ti in t]...)

u = u_true + u_noise
z = z_true + z_noise

k_loss = 2001:4000

gps_sig = ones(size(t)) .|> Bool
mag_sig = ones(size(t)) .|> Bool
gps_sig[k_loss] = zeros(length(k_loss)) .|> Bool

x, P2, K2 = kalman(t, 10, u, z, Q, R, gps_sig, mag_sig)

trayectoria_xy(x, z_true, k_loss)

trayectoria_t(t, x, z_true, k_loss)

plot_error(t, x, z_true, P2, k_loss)

gps_sig = ones(size(t)) .|> Bool
mag_sig = ones(size(t)) .|> Bool
mag_sig[k_loss] = zeros(length(k_loss)) .|> Bool

x, P2, K2 = kalman(t, 10, u, z, Q, R, gps_sig, mag_sig)

([20.795148915177673 20.795117125888112 … 20.242389488641933 20.242765609415997; 0.0 -0.0006357857912123561 … 0.003717783652726088 0.003804631828543173; … ; 0.0 -0.0004717493715834802 … 0.2581043301022885 0.25789636057409177; 1.570159913381644 1.5716896101676954 … 7.852512900119905 7.853581883708354], [39.0625 0.0 … 0.0 0.0; 0.0 39.0625 … 0.0 0.0; … ; 0.0 0.0 … 39.0625 0.0; 0.0 0.0 … 0.0 6.283185307179586;;; 39.45312500016 3.9062500032 … 1.2916044610942532e-29 0.0; 3.9062500032 39.062500064 … 5.243052355554181e-27 0.0; … ; 1.9940497855492672e-28 -3.284111285018992e-27 … 39.062500064 0.0; 0.0 0.0 … 0.0 6.283185334595154;;; 40.6250000016 7.8125000128 … 5.926439159334028e-28 0.0; 7.8125000128 39.062500127999996 … -3.1064992711983e-27 0.0; … ; 1.7851743431851958e-28 1.3947598470494988e-27 … 39.062500127999996 0.0; 0.0 0.0 … 0.0 6.283185362010721;;; … ;;; 0.15972723750735476 0.00202037050907998 … -6.442695314285448e-22 0.0; 0.0020203705090799796 5.085414131495572e-5 … -2.78373821095346e-23 0.0; … ; 1.6610559400219101e-21 6.533362657764144e-23 … 5.085414131495572e-5 0.0; 0.0 0.0 … 0.0 3.575038139502067e-7;;; 0.1561315593807332 0.0019748610285924453 … -6.248445440813035e-22 0.0; 0.001974861028592445 5.027814126896574e-5 … -2.7409106120205694e-23 0.0; … ; 1.609295372519169e-21 6.426784175872226e-23 … 5.027814126896574e-5 0.0; 0.0 0.0 … 0.0 1.1076370392297255e-7;;; 0.1565270345278644 0.0019798920427193415 … -6.275858592719784e-22 0.0; 0.001979892042719341 5.034214126896574e-5 … -2.7409370805685353e-23 0.0; … ; 1.6157218015671055e-21 6.427381424130635e-23 … 5.034214126896574e-5 0.0; 0.0 0.0 … 0.0 1.3817927170377632e-7], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7623134290242808  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.7131175945847074, 0.0])

trayectoria_xy(x, z_true, k_loss)

trayectoria_t(t, x, z_true, k_loss)

plot_error(t, x, z_true, P2, k_loss)